#include "IDroplet.h"

// Constructors and Destructor
IDroplet::IDroplet(ObjectPhysicsData *objPhysics)
{
	this->objPhysics = objPhysics;
	actData		= (DropletActuatorData *)malloc(sizeof(DropletActuatorData));
	senseData	= (DropletSensorData *)malloc(sizeof(DropletSensorData));
	commData	= (DropletCommData *)malloc(sizeof(DropletCommData));
	compData	= (DropletCompData *)malloc(sizeof(DropletCompData));
}
	
IDroplet::~IDroplet()
{
	free(objPhysics);
	free(actData);
	free(senseData);
	free(commData);
	free(compData);

	objPhysics = NULL;
	actData = NULL;
	senseData = NULL;
	commData = NULL;
	compData = NULL;
}

DS_RESULT IDroplet::_InitPhysics(
	SimPhysicsData *simPhysics, 
	std::pair<float, float> startPosition,
	float startAngle)
{
	// Set up the bullet physics object for this Droplet
	btCollisionShape *colShape = simPhysics->collisionShapes->at(objPhysics->colShapeIndex);

	bool isDynamic = (objPhysics->mass != 0.0);
	colShape->calculateLocalInertia(objPhysics->mass, objPhysics->localInertia);

	// Set the Droplet's initial position in the world. Remember that bullet uses a Y-Up coord system
	btTransform startTransform;
	startTransform.setIdentity();
	startTransform.setOrigin(btVector3(
		btScalar(startPosition.first), 
		0.1f,
		btScalar(startPosition.second))
	); // TODO : This may be wrong. Look into it!

	// Set this Droplet's angular orientation using input parameter startAngle
	btQuaternion rotationQuat; 
	rotationQuat.setRotation(btVector3(
			btScalar(0.0), 
			btScalar(1.0),
			btScalar(0.0)
		), 
		btScalar(startAngle)
	);
	startTransform.setRotation(rotationQuat);

	// Add this Droplet as a rigid body in the physics sim
	btDefaultMotionState* motionState = new btDefaultMotionState(startTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(
		objPhysics->mass, 
		motionState, 
		colShape, 
		objPhysics->localInertia
	);
	rbInfo.m_friction = objPhysics->friction;
	rbInfo.m_linearDamping = 0.9f;		// Add a damping coefficient

	btRigidBody *body = new btRigidBody(rbInfo);
	simPhysics->dynWorld->addRigidBody(body);

	// Set the Droplet's ID
	objPhysics->_worldID = simPhysics->_physicsWorldObjCounter++;
	compData->dropletID = objPhysics->_worldID + DROPLET_ID_START;

	return DS_SUCCESS;
}

// Droplet Subsystems Setup Functions
void IDroplet::ResetAllSystems() 
{
	ResetMotors();
	ResetRGBLED();
	ResetRGBSensor();
	ResetIRSensor();
	ResetIRLED();
}

void IDroplet::ResetRGBLED() 
{
	actData->rOut = 0;
	actData->gOut = 0;
	actData->bOut = 0;
}

void IDroplet::ResetIRLED() 
{
	// TODO : Communication and Range/Bearing systems still to be implemented
}

void IDroplet::ResetIRSensor() 
{
	// TODO : Communication and Range/Bearing systems still to be implemented
}

void IDroplet::ResetRGBSensor() 
{
	senseData->rIn = 0;
	senseData->gIn = 0;
	senseData->bIn = 0;
}

void IDroplet::ResetMotors() 
{
	actData->currMoveDir			= MOVE_OFF;
	actData->currTurnDir			= TURN_OFF;
	actData->moveTimeRemaining		= 0;
	actData->rotateTimeRemaining	= 0;
}

uint16_t IDroplet::GetDropletID()
{
	return compData->dropletID;
}

void IDroplet::SeedRandom(int32_t seed)
{
	srand(seed);
}

uint8_t IDroplet::GetRandom(void)
{
	return static_cast<uint8_t>(255 * ((float)rand() / RAND_MAX));
}

// Droplet Motion Subsystem Functions
void IDroplet::Rotate(uint8_t dur, turn_direction dir)
{
	actData->rotateTimeRemaining = static_cast<float>(dur) * 1000.0f;
	actData->currTurnDir = dir;
}

void IDroplet::Move(uint8_t dur, move_direction dir)
{
	actData->moveTimeRemaining = static_cast<float>(dur) * 1000.0f;
	actData->currMoveDir = dir;
}

void IDroplet::CancelRotate() 
{
	actData->rotateTimeRemaining = 0;
	actData->currTurnDir = TURN_OFF;
}

void IDroplet::CancelMove() 
{
	actData->moveTimeRemaining = 0;
	actData->currMoveDir = MOVE_OFF;
}

uint8_t IDroplet::IsRotating() 
{
	if(actData->rotateTimeRemaining <= 0)
		return 0;
	else
		return 1;
}

uint8_t IDroplet::IsMoving()
{
	if(actData->moveTimeRemaining <= 0)
		return 0;
	else
		return 1;
}
	
void IDroplet::SetRGB(uint8_t r, uint8_t g, uint8_t b)
{
	actData->rOut = r;
	actData->gOut = g;
	actData->bOut = b;
}

void IDroplet::GetRGB(uint8_t *r, uint8_t *g, uint8_t *b)
{
	*r = senseData->rIn;
	*g = senseData->gIn;
	*b = senseData->bIn;
}

void IDroplet::DropletInit(void) { return; }
void IDroplet::DropletMainLoop(void) { return; }